#include "motor.h"

int get_motor_pair(int m)
{
	switch (m){
	case 0:
		return 1;
	case 1: 
		return 0;
	case 2:
		return 3;
	case 3:
		return 2;
	}
	return 0;
}



void rotate_trigonometric(int leg)
{
	switch(leg) {
	case 0:
		CLR_BIT(mPORT0, m0_In1);
		SET_BIT(mPORT0, m0_In2);
		break;
	case 1:
		CLR_BIT(mPORT1, m1_In1);
		SET_BIT(mPORT1, m1_In2);
		break;
	case 2:
		CLR_BIT(mPORT2, m2_In1);
		SET_BIT(mPORT2, m2_In2);
		break;
	case 3:
		CLR_BIT(mPORT3, m3_In1);
		SET_BIT(mPORT3, m3_In2);
		break;
	}
}

void rotate_clockwise(int leg)
{
	switch(leg) {
	case 0:
		CLR_BIT(mPORT0, m0_In2);
		SET_BIT(mPORT0, m0_In1);
		break;
	case 1:
		CLR_BIT(mPORT1, m1_In2);
		SET_BIT(mPORT1, m1_In1);
		break;
	case 2:
		CLR_BIT(mPORT2, m2_In2);
		SET_BIT(mPORT2, m2_In1);
		break;
	case 3:
		CLR_BIT(mPORT3, m3_In2);
		SET_BIT(mPORT3, m3_In1);
		break;
	}
}

void stop_motor(int leg)
{
	switch(leg) {
	case 0:
		CLR_BIT(mPORT0, m0_In2);
		CLR_BIT(mPORT0, m0_In1);
		break;
	case 1:
		CLR_BIT(mPORT1, m1_In2);
		CLR_BIT(mPORT1, m1_In1);
		break;
	case 2:
		CLR_BIT(mPORT2, m2_In2);
		CLR_BIT(mPORT2, m2_In1);
		break;
	case 3:
		CLR_BIT(mPORT3, m3_In2);
		CLR_BIT(mPORT3, m3_In1);
		break;
	}
}

void stop_motor_pair(int leg)
{
	stop_motor(leg);
	stop_motor(get_motor_pair(leg));
}

void stop_motors()
{
	for(int i = 0; i < 4; i++)
		stop_motor(i);
	//little_wait();
}

void move(int leg, int dir)
{
	switch(dir) {
	case ROT_TRIGONOMETRIC:
		rotate_trigonometric(leg);
		break;
	case ROT_STOPPED:
		stop_motor(leg);
		break;
	case ROT_CLOCKWISE:
		rotate_clockwise(leg);
		break;
	}
}

